#include "srv_sensor.h"
#include "drv_led.h"


static double direction_error = 0;

static uint8_t power_tabs[3][3]={
    {A_POWER_0, A_POWER_1, A_POWER_2},
    {B_POWER_0, B_POWER_1, B_POWER_2},
    {C_POWER_0, C_POWER_1, C_POWER_2}
};

static uint8_t power_tabs_index=0;

void ist_fix_power_table(uint8_t expect_pow_num)
{
    if(expect_pow_num<0 ||expect_pow_num>2)
    {
        return ;
    }
    power_tabs_index = expect_pow_num;
}

static int left_status[] = {
    0,
    0,
    0,
    0,
};
static int right_status[] = {
    0,
    0,
    0,
    0,
};

static int _bitcount(unsigned int n)
{
    int count = 0;
    while (n)
    {
        count++;
        n &= (n - 1);
    }
    return count;
}

static void _analyse_direction_error(void *param)
{
    double t_dir_err = 0;
    rt_uint8_t led_status = ist_led_get();
    // static uint8_t mid_count =0;
    int i;

    int count = _bitcount(led_status);
    if (count == 0)
    {
        return;
    }

    // 左侧
    for (i = 0; i < 4; i++)
    {
        if (led_status & (1 << (i + 4)))
        {
            // left_status[i] = SHADOW_LOST_TIME;
            left_status[i] = 1;
        }
        else
        {
            left_status[i] = 0;
        }
    }
    // 右侧
    for (i = 0; i < 4; i++)
    {
        if (led_status & (1 << (3 - i)))
        {
            // right_status[i] = SHADOW_LOST_TIME;
            right_status[i] = 1;
        }
        else
        {
            right_status[i] = 0;
        }
    }

    for (i = 0; i < 3; i++)
    {
        t_dir_err -=  power_tabs[power_tabs_index][i] * left_status[i];
        t_dir_err += power_tabs[power_tabs_index][i] * right_status[i];
    }
    direction_error = t_dir_err;
}

static void ist_sensor_init(void)
{
    // TODO
    ist_led_init();

    rt_timer_t timer = rt_timer_create("sensor", _analyse_direction_error, RT_NULL, 5, RT_TIMER_FLAG_SOFT_TIMER | RT_TIMER_FLAG_PERIODIC);
    rt_timer_start(timer);
}

double ist_sensor_get_direction_error(void)
{
    return direction_error;
}


static uint8_t power_tab_fix_temp[3];
// static uint8_t table_index_temp;

void ist_power_down(uint8_t level)
{
    // table_index_temp = tabal_index;
    // power_tab_fix_temp = power_tabs[power_tabs_index][tabal_index];
    // power_tabs[power_tabs_index][tabal_index] = 0;

    for(int i=0; i<3;i++)
    {
        power_tab_fix_temp[i] = power_tabs[power_tabs_index][i];
    }
    switch(level)
    {
        case 1:
            power_tabs[power_tabs_index][2] = 0;
            break;
        case 2:
            power_tabs[power_tabs_index][1] = power_tabs[power_tabs_index][0];
            power_tabs[power_tabs_index][2] = 0;
            break;
        default:
            break;
    }
}
void ist_power_up(void)
{
    // power_tabs[power_tabs_index][table_index_temp] = power_tab_fix_temp;
    for(int i=0;i<3;i++)
    {
        power_tabs[power_tabs_index][i] = power_tab_fix_temp[i];
    }
}


INIT_COMPONENT_EXPORT(ist_sensor_init);
